#include <math.h>
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <string>
#include <cstring>

#include <webots/Robot.hpp>
#include <webots/Servo.hpp>
#include <webots/DifferentialWheels.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Camera.hpp>
#include <webots/Display.hpp>

#include <ros/ros.h>
#include <sensor_msgs/Image.h>

// %EndTag(MSG_HEADER)%



using namespace std;
using namespace webots;



class RWCamera
{
public:
  RWCamera();
  void init(webots::Robot*,ros::NodeHandle*,string);
  ~RWCamera();

  void iterate();
private:
  double time_step;

  double rgbcam_width,rgbcam_height;

  Camera* RGBCAM;

  ros::NodeHandle* n;
  ros::Publisher rgbcam_image_color_pub;
  ros::Publisher rgbcam_image_mono_pub;

  sensor_msgs::Image RgbcamImageColor;
  sensor_msgs::Image RgbcamImageMono;

};
